#include "define.h"

static struct ECAN_REGS ECanaShadow;
void MailBox0Tx(void)
{
	//index0
	ECanaMboxes.MBOX0.MDL.byte.BYTE0 = 0x1;
	ECanaMboxes.MBOX0.MDL.byte.BYTE1 = 0x0;
	ECanaMboxes.MBOX0.MDL.byte.BYTE2 = g_CanMsg.MotorV & 0xFF;
	ECanaMboxes.MBOX0.MDL.byte.BYTE3 = (g_CanMsg.MotorV >> 8) & 0xFF;
	ECanaMboxes.MBOX0.MDH.byte.BYTE4 = g_CanMsg.MotorA & 0xFF;
	ECanaMboxes.MBOX0.MDH.byte.BYTE5 = (g_CanMsg.MotorA >> 8) & 0xFF;
	ECanaMboxes.MBOX0.MDH.byte.BYTE6 = g_CanMsg.SpeedFeed & 0xFF;
	ECanaMboxes.MBOX0.MDH.byte.BYTE7 = (g_CanMsg.SpeedFeed >> 8) & 0xFF;
	// Set TRS for mailbox
	ECanaShadow.CANTRS.all = 0;
	ECanaShadow.CANTRS.bit.TRS0 = 1;
	ECanaRegs.CANTRS.all = ECanaShadow.CANTRS.all;
	DELAY_US(500);
	// Wait for TA0 bit to be set
	ECanaShadow.CANTA.all = ECanaRegs.CANTA.all;
	if(ECanaShadow.CANTA.bit.TA0 == 1 )
	{
		//todo:no ack
	}
	// Clear TA0
	ECanaShadow.CANTA.all = 0;
	ECanaShadow.CANTA.bit.TA0 = 1;
	ECanaRegs.CANTA.all = ECanaShadow.CANTA.all;

	DELAY_US(500);

	//index1
	ECanaMboxes.MBOX0.MDL.byte.BYTE0 = 0x1;
	ECanaMboxes.MBOX0.MDL.byte.BYTE1 = 0x1;
	ECanaMboxes.MBOX0.MDL.byte.BYTE2 = g_CanMsg.DrvState & 0xFF;
	ECanaMboxes.MBOX0.MDL.byte.BYTE3 = (g_CanMsg.DrvState >> 8) & 0xFF;
	ECanaMboxes.MBOX0.MDH.byte.BYTE4 = g_CanMsg.ErrState.all & 0xFF;
	ECanaMboxes.MBOX0.MDH.byte.BYTE5 = (g_CanMsg.ErrState.all >> 8) & 0xFF;
	ECanaMboxes.MBOX0.MDH.byte.BYTE6 = 0;
	ECanaMboxes.MBOX0.MDH.byte.BYTE7 = 0;
	// Set TRS for mailbox
	ECanaShadow.CANTRS.all = 0;
	ECanaShadow.CANTRS.bit.TRS0 = 1;
	ECanaRegs.CANTRS.all = ECanaShadow.CANTRS.all;
	DELAY_US(500);
	// Wait for TA0 bit to be set
	ECanaShadow.CANTA.all = ECanaRegs.CANTA.all;
	if(ECanaShadow.CANTA.bit.TA0 == 1 )
	{
		//todo:no ack
	}
	// Clear TA0
	ECanaShadow.CANTA.all = 0;
	ECanaShadow.CANTA.bit.TA0 = 1;
	ECanaRegs.CANTA.all = ECanaShadow.CANTA.all;
}

void MailBox1Tx(Uint16 RegAddr)
{
	//index0
	ECanaMboxes.MBOX1.MDL.byte.BYTE0 = 0x2;
	ECanaMboxes.MBOX1.MDL.byte.BYTE1 = RegAddr;
	switch(RegAddr)
	{
		case 0x10:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.MotorV & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.MotorV >> 8) & 0xFF;
			break;
		}
		case 0x11:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.MotorA & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.MotorA >> 8) & 0xFF;
			break;
		}
		case 0x14:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.SpeedFeed & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.SpeedFeed >> 8) & 0xFF;
			break;
		}
		case 0x15:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.DrvState & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.DrvState >> 8) & 0xFF;
			break;
		}
		case 0x16:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.Pwm & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.Pwm >> 8) & 0xFF;
			break;
		}
		case 0x41:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.ErrState.all & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.ErrState.all >> 8) & 0xFF;
			break;
		}
		case 0x43:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.MotorOverI & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.MotorOverI >> 8) & 0xFF;
			break;
		}
		case 0x44:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.Mode & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.Mode >> 8) & 0xFF;
			break;
		}
		case 0x45:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.CommPeriod & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.CommPeriod >> 8) & 0xFF;
			break;
		}
		case 0x52:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.SpeedRef & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.SpeedRef >> 8) & 0xFF;
			break;
		}
		case 0x53:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.Control.all & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.Control.all >> 8) & 0xFF;
			break;
		}
		case 0x55:
		{
			ECanaMboxes.MBOX1.MDL.byte.BYTE2 = g_CanMsg.MotorUnderV & 0xFF;
			ECanaMboxes.MBOX1.MDL.byte.BYTE3 = (g_CanMsg.MotorUnderV >> 8) & 0xFF;
			break;
		}
		default:
		{
			ECanaMboxes.MBOX1.MDL.word.LOW_WORD = 0;
			break;
		}
	}

	// Set TRS for mailbox
	ECanaShadow.CANTRS.all = 0;
	ECanaShadow.CANTRS.bit.TRS1 = 1;
	ECanaRegs.CANTRS.all = ECanaShadow.CANTRS.all;

	DELAY_US(500);
	// Wait for TA0 bit to be set
	ECanaShadow.CANTA.all = ECanaRegs.CANTA.all;
	if(ECanaShadow.CANTA.bit.TA1 == 1 )
	{
		//todo:no ack
	}

	// Clear TA1
	ECanaShadow.CANTA.all = 0;
	ECanaShadow.CANTA.bit.TA1 = 1;
	ECanaRegs.CANTA.all = ECanaShadow.CANTA.all;
}


void MailBox3Rx(void)
{
	if(ECanaRegs.CANRMP.bit.RMP3 == 1)
	{
		if( (ECanaMboxes.MBOX3.MSGCTRL.bit.DLC == 2) && (ECanaMboxes.MBOX3.MDL.byte.BYTE0 == 0x20) )
		{
			MailBox1Tx(ECanaMboxes.MBOX3.MDL.byte.BYTE1);
		}

		if( (ECanaMboxes.MBOX3.MSGCTRL.bit.DLC == 4) && (ECanaMboxes.MBOX3.MDL.byte.BYTE0 == 0x10) )
		{
			switch(ECanaMboxes.MBOX3.MDL.byte.BYTE1)
			{
				case 0x41:
				{
					if( (ECanaMboxes.MBOX3.MDL.byte.BYTE2 == 0) && (ECanaMboxes.MBOX3.MDL.byte.BYTE3 == 0) )
					{
						g_CanMsg.ErrState.all = 0;
					}
					break;
				}
				case 0x43:
				{
					//g_CanMsg.MotorOverI = ECanaMboxes.MBOX3.MDL.byte.BYTE2 | (ECanaMboxes.MBOX3.MDL.byte.BYTE3 << 8);
					break;
				}
				case 0x45:
				{
					g_CanMsg.CommPeriod = ECanaMboxes.MBOX3.MDL.byte.BYTE2 | (ECanaMboxes.MBOX3.MDL.byte.BYTE3 << 8);
					break;
				}
				case 0x52:
				{
					g_CanMsg.SpeedRef = ECanaMboxes.MBOX3.MDL.byte.BYTE2 | (ECanaMboxes.MBOX3.MDL.byte.BYTE3 << 8);
					break;
				}
				case 0x53:
				{
					g_CanMsg.Control.all = ECanaMboxes.MBOX3.MDL.byte.BYTE2 | (ECanaMboxes.MBOX3.MDL.byte.BYTE3 << 8);
					break;
				}
				case 0x55:
				{
					//g_CanMsg.MotorUnderV = ECanaMboxes.MBOX3.MDL.byte.BYTE2 | (ECanaMboxes.MBOX3.MDL.byte.BYTE3 << 8);
					break;
				}
				default:
				{
					break;
				}
			}

		}

		ECanaShadow.CANRMP.all = 0;
		ECanaShadow.CANRMP.bit.RMP3 = 1;
		ECanaRegs.CANRMP.all = ECanaShadow.CANRMP.all;
	}
}
